#include "geometry.h"

//*********************************
Matrix44::Matrix44()
{
	clear();
}


void Matrix44::clear()
{
	memset(m, 0, 16*sizeof(float));
}

void Matrix44::setIdentity()
{
	m[0]=1; m[4]=0; m[8]=0; m[12]=0;
	m[1]=0; m[5]=1; m[9]=0; m[13]=0;
	m[2]=0; m[6]=0; m[10]=1; m[14]=0;
	m[3]=0; m[7]=0; m[11]=0; m[15]=1;
}

void Matrix44::setZeros()
{
	m[0]=0; m[4]=0; m[8]=0; m[12]=0;
	m[1]=0; m[5]=0; m[9]=0; m[13]=0;
	m[2]=0; m[6]=0; m[10]=0; m[14]=0;
	m[3]=0; m[7]=0; m[11]=0; m[15]=0;
}



VECTOR Matrix44::rotateVector(const VECTOR& v)
{
	Matrix44 temp = *this;
	temp.m[12] = 0.0;
	temp.m[13] = 0.0;
	temp.m[14] = 0.0;
	return temp * v;
}



//To create a rotation matrix
void Matrix44::setRotation( float angle_in_rad, const VECTOR& axis  )
{
	clear();
	VECTOR axis_n = axis;
	axis_n.normalize();

	float c = cos( angle_in_rad );
	float s = sin( angle_in_rad );
	float t = 1 - c;

	M[0][0] = t * axis_n.x * axis_n.x + c;
	M[0][1] = t * axis_n.x * axis_n.y - s * axis_n.z;
	M[0][2] = t * axis_n.x * axis_n.z + s * axis_n.y;

	M[1][0] = t * axis_n.x * axis_n.y + s * axis_n.z;
	M[1][1] = t * axis_n.y * axis_n.y + c;
	M[1][2] = t * axis_n.y * axis_n.z - s * axis_n.x;

	M[2][0] = t * axis_n.x * axis_n.z - s * axis_n.y;
	M[2][1] = t * axis_n.y * axis_n.z + s * axis_n.x;
	M[2][2] = t * axis_n.z * axis_n.z + c;

	M[3][3] = 1.0f;
}

//Multiply a matrix by another and returns the result
Matrix44 Matrix44::operator*(const Matrix44& matrix) const
{
	Matrix44 ret;

	unsigned int i,j,k;
	for (i=0;i<4;i++) 	
	{
		for (j=0;j<4;j++) 
		{
			ret.M[i][j]=0.0;
			for (k=0;k<4;k++) 
				ret.M[i][j] += M[i][k] * matrix.M[k][j];
		}
	}

	return ret;
}

VECTOR operator * (const VECTOR& a, float v) 
{
	return VECTOR(a.x * v, a.y * v, a.z * v);
}

//Multiplies a vector by a matrix and returns the new vector
VECTOR operator * (const Matrix44& matrix, const VECTOR& v) 
{   
	float x = matrix.m[0] * v.x + matrix.m[4] * v.y + matrix.m[8] * v.z + matrix.m[12]; 
	float y = matrix.m[1] * v.x + matrix.m[5] * v.y + matrix.m[9] * v.z + matrix.m[13]; 
	float z = matrix.m[2] * v.x + matrix.m[6] * v.y + matrix.m[10] * v.z + matrix.m[14];
	return VECTOR(x,y,z);
}